#!/usr/bin/env python
import roslib; roslib.load_manifest('robcar_ros')
import time
import numpy as np
import matplotlib.pyplot as plt
import matplotlib.mlab as mlab

import rospy
from robcar_ros.msg import prob

pose_prob = prob()

fig = plt.figure()
ax = fig.add_subplot(211)

ax.set_xlabel('Position')
ax.set_ylabel('Probability')
ax.set_xlim(1, 8)
ax.set_ylim(0, 1.1)
ax.grid(True)

la, = ax.plot([0,0,0,0,0,0,0,0],[0,0,0,0,0,0,0,0])
fig.canvas.draw()


def draw_prob(data):
	global pose_prob
	global la
	global fig
	
	print "d=",data
	print "d.p=",list(data.prob)
	
	la.set_ydata(list(data.prob))
	fig.canvas.draw()
	
def listener():
	rospy.init_node('pose_prob_viewer', anonymous=True)
	rospy.Subscriber("prob_data", prob, draw_prob)
	rospy.spin()

if __name__ == '__main__':
    try:
    	listener()
        
    except rospy.ROSInterruptException:
    	pass


